#include "pose_estimate.h"
extern "C"
{
    #include "pose.h"
    #include "homography.h"
    #include "zarray.h"
    #include "matd.h"
    #include "stdio.h"
}

const char* MAT_FMT = "%12f, ";

matd_t* homography_from_corners_(const double corners[][2]) {

    zarray_t* correspondences = zarray_create(sizeof(float[4]));
    
    for (int i=0; i<4; ++i) {
        
        float corr[4];
        
        corr[0] = (i==0 || i==3) ? -1 : 1;
        corr[1] = (i==0 || i==1) ? -1 : 1;
        corr[2] = corners[i][0];
        corr[3] = corners[i][1];
        
        zarray_add(correspondences, &corr);

    }


    matd_t* H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD);

    zarray_destroy(correspondences);

    

    return H;

}

void estimate_pose_from_vertex(const double corners[][2], pose_t* pose, const double tagsize,\
    const double fx, const double fy, const double cx, const double cy)
{
    matd_t* H = homography_from_corners_(corners);
    
    // printf("H:\n");
    // matd_print(H, MAT_FMT);
    // printf("\n");

    matd_t* M = pose_from_homography(H, fx, fy, cx, cy, tagsize, 1.0,
                                     NULL, NULL, NULL);

    mat4_to_rvec_tvec(M, pose->rvec, pose->tvec);
    
    matd_destroy(H);
    matd_destroy(M);
}


